#include <stdio.h>
#include <stdlib.h>
#include "MoveMode.h"
#include "../Method/PID.h"
#define BAISE_SPEED 60
POS car_pos={
        .x=0.0f,
        .y=0.0f
};
bool Move_stop_black()
{
    HD_read_dates();
    int flag=0;
    for (int i = 0; i < 8; i++) {
        if (HD_dates[i]==0) {
            flag++;
        }
    }
    if (flag>=2)
    {
        PositionPID_Clear();
        return true;
    }
    return false;
}
bool Move_Trace(float speed)
{
    float s_yaw=Yaw;
    static float pid_pwm;
    while (1) {
        HD_read_dates();

        float hd=calculate_offset_switch();
        if (hd==255)
            return true;
        if(hd!=-25)
            pid_pwm = PositionPID(-hd);
//      printf("%d\n",calculate_offset());
        Emm_V5_Control_dir_speed(pid_pwm, speed);




    }


    }
//    HAL_Delay(500);
//    Emm_V5_Vel_Control(3,0,BAISE_SPEED,0,0);
//     Emm_V5_Vel_Control(4,1,BAISE_SPEED,0,0);

// 计算偏差的通用函数
float calculate_deviation(float target_angle) {
    float deviation = Yaw - target_angle;
    if (deviation > 180.0f) {
        deviation -= 360.0f; // 调整到 -180 到 180 范围
    } else if (deviation < -180.0f) {
        deviation += 360.0f; // 调整到 -180 到 180 范围
    }
    return deviation;
}
bool Move_To_dir(int dir,float speed,bool start)
{
    while (1) {
        if(!start&&Move_stop_black())
        {
            //CascadeCalc_Clear();
            return true;
        }
        float deviation_a = calculate_deviation(dir); // 计算偏差
        float pid_angle = PID_CascadeCalc(deviation_a);        // 通过 PID 控制器计算控制量

        if (!start)
            Emm_V5_Control_dir_speed(-pid_angle, speed);
        else
            Emm_V5_Control_dir_speed(-pid_angle, speed);

        if (abs(deviation_a)<=0.1&&start)
        {
            Emm_V5_Stop_Now(0,0);
            //CascadeCalc_Clear();
            return false;

        }
        //printf("%.2f,%.2f\n",car_pos.x,car_pos.y);

    }
}
bool Move_To_distance(int dir,float speed,int time)
{
    uint32_t start_time = HAL_GetTick();     //time 为毫秒单位

    while ((HAL_GetTick() - start_time) < time) {
        float deviation_a = calculate_deviation(dir);      // 当前角度偏差
        float pid_angle = PID_CascadeCalc(deviation_a);    // PID计算
        Emm_V5_Control_dir_speed(-pid_angle, speed);       // 控制方向和速度
    }

    Emm_V5_Control_dir_speed(0, 0); // 到时间后停止
    return true;


}
bool Move_To_dir_dis(int dir,float speed,int d)
{
    Emm_V5_Reset_CurPos_To_Zero(3);
    Emm_V5_Reset_CurPos_To_Zero(4);
    float now_pos=(Emm_read_pos(3)+Emm_read_pos(4))/2;
    while(1)
    {
        float new_pos=(Emm_read_pos(3)+Emm_read_pos(4))/2;
        float deviation_a = calculate_deviation(dir);      // 当前角度偏差
        float pid_angle = PID_CascadeCalc(deviation_a);    // PID计算
        Emm_V5_Control_dir_speed(-pid_angle, speed);       // 控制方向和速度
        if (new_pos-now_pos>d)
        {
            Emm_V5_Control_dir_speed(0, 0); // 到时间后停止
            return true;
        }
    }

}

void Mode_A(){
    Move_To_dir(0,120,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(70);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_To_dir(180,120,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_Trace(70);
    Move_To_dir(0,5,1);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
}
void Mode_B(){
    Move_To_distance(-40,70,4000);
    Move_To_dir(0,0,1);
    Move_To_dir(5,50,0);//0是要巡线，找到线停止。
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);
    Emm_V5_Stop_Now(0,0);
    Move_To_dir(180,0,1);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_To_distance(180+40,70,3900);
    Move_To_dir(180,0,1);
    Move_To_dir(180,70,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);
    Emm_V5_Stop_Now(0,0);
    Move_To_dir(10,0,1);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
}
void Mode_B1(int i)
{
    Move_To_distance(-40,70,3700-100*i);
    Move_To_dir(0,0,1);
    Move_To_dir(5,50,0);//0是要巡线，找到线停止。
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);
    Emm_V5_Stop_Now(0,0);
    Move_To_dir(180,0,1);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_To_distance(180+40,70,3900+85*i);
    Move_To_dir(180,0,1);
    Move_To_dir(180,70,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);
    Emm_V5_Stop_Now(0,0);
    Move_To_dir(10,0,1);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
}
void Mode_B2()
{
    /*第一圈*/

    Move_To_dir(0,70,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(70);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_To_dir(180,70,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_Trace(70);
    Move_To_dir(0,5,1);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

//    Move_To_dir(-39,0,1);
//    Move_To_dir_dis(-39,70,1650);
//    Move_To_dir(0,0,1);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();
//    Move_Trace(50);//循迹
//    Emm_V5_Stop_Now(0,0);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();
//
//    Move_To_dir(180,0,1);
//    Move_To_dir(180+35,0,1);
//    Move_To_dir_dis(180+35,70,1600);
//    Move_To_dir(180,0,1);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();
//    Move_Trace(50);//循迹
//    Emm_V5_Stop_Now(0,0);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();

    /*第二圈*/
    Move_To_dir(0,70,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(70);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_To_dir(180,70,0);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_Trace(70);
    Move_To_dir(0,5,1);
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

//    Move_To_dir(0,0,1);
//
//    Move_To_dir(-37,0,1);
//    Move_To_dir_dis(-37,70,1599);
//    Move_To_dir(0,0,1);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();
//    Move_Trace(50);//循迹
//    Emm_V5_Stop_Now(0,0);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();
//
//    Move_To_dir(180,0,1);
//    Move_To_dir(180+35,0,1);
//    Move_To_dir_dis(180+35,70,1600);
//    Move_To_dir(180,0,1);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();
//    Move_Trace(50);//循迹
//    Emm_V5_Stop_Now(0,0);
//    Open_Buzzer();
//    dwt_delay_ms(200);
//    init_Buzzer();

    /*第三圈*/
    Move_To_dir(0,0,1);
    Move_To_dir(-40,0,1);
    Move_To_dir_dis(-40,70,1500);
    Move_To_dir(0,20,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);//循迹
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_To_dir(180,0,1);
    Move_To_dir(180+35,0,1);
    Move_To_dir_dis(180+35,70,1600);
    Move_To_dir(180,0,1);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);//循迹
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    /*第四圈*/
    Move_To_dir(0,0,1);
    Move_To_dir(-40,0,1);
    Move_To_dir_dis(-40,70,1500);
    Move_To_dir(0,20,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);//循迹
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();

    Move_To_dir(180,0,1);
    Move_To_dir(180+35,0,1);
    Move_To_dir_dis(180+35,70,1600);
    Move_To_dir(180,0,1);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Move_Trace(40);//循迹
    Emm_V5_Stop_Now(0,0);
    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();


}
void Mode_C() {
    int i=0;
    Mode_B();
    i=i+1;
    Mode_B1(i);
    i=i+1;
    Mode_B1(i);
    i=i+1;
    Mode_B1(i);
    Move_To_dir(0,0,1);
}



void Mode_D(){
    Move_To_dir(0,80,0);

    Open_Buzzer();
    dwt_delay_ms(200);
    init_Buzzer();
    Emm_V5_Stop_Now(0,0);

}

#define Gyro_Z_lim 5.0
float next_d;
float d;
void Get_XY()
{

    if (fabs(Gyro_Z)>Gyro_Z_lim)//圆弧运动
    {
        float v=Emm_read_speed(3)+Emm_read_speed(4);
        if (next_d-d<-1) {
            car_pos.x += (float) (fabs(Gyro_Z / Acc_Z) * cos(Yaw * (M_PI / 180.0)));
            car_pos.y += (float) (fabs(Gyro_Z / Acc_Z) * sin(Yaw * (M_PI / 180.0)));
        }else if(next_d-d>1)
        {
            car_pos.x -= (float) (fabs(Gyro_Z / Acc_Z) * cos(Yaw * (M_PI / 180.0)));
            car_pos.y -= (float) (fabs(Gyro_Z / Acc_Z) * sin(Yaw * (M_PI / 180.0)));
        }
    }else{//直线运动
        d=(Emm_read_pos(3)+Emm_read_pos(4))/2;
        if (next_d-d<-1) {
            car_pos.x += (float) (fabs(M_PI * LR / (next_d - d)) * cos(Yaw * (M_PI / 180.0)));
            car_pos.y += (float) (fabs(M_PI * LR / (next_d - d)) * sin(Yaw * (M_PI / 180.0)));
        } else if(next_d-d>1)
        {
            car_pos.x -= (float) (fabs(M_PI * LR / (next_d - d)) * cos(Yaw * (M_PI / 180.0)));
            car_pos.y -= (float) (fabs(M_PI * LR / (next_d - d)) * sin(Yaw * (M_PI / 180.0)));
        }
        next_d=d;
    }

}